import rclpy
from rclpy.node import Node
from nav_msgs.msg import Path
from geometry_msgs.msg import Twist
from rclpy.executors import MultiThreadedExecutor
from rclpy.callback_groups import ReentrantCallbackGroup
from algoithm import VectorPathFollowingAlgorithm
from ros_interface import ROSInterface

class VectorPathFollower(Node):
    def __init__(self):
        super().__init__('vector_path_follower')
        
        # 参数设置
        self.declare_parameter('linear_speed', 0.2)  # 线速度 m/s
        self.declare_parameter('angular_speed_max', 0.5)  # 最大角速度 rad/s
        self.declare_parameter('lookahead_distance', 0.3)  # 前视距离 m
        self.declare_parameter('goal_threshold', 0.1)  # 目标点到达阈值 m
        self.declare_parameter('orientation_threshold', 0.1)  # 方向对准阈值 rad
        self.declare_parameter('base_frame', 'base_link')  # 机器人基坐标系
        self.declare_parameter('global_frame', 'map')  # 全局坐标系
        
        # 获取参数
        params = {
            'linear_speed': self.get_parameter('linear_speed').value,
            'angular_speed_max': self.get_parameter('angular_speed_max').value,
            'lookahead_distance': self.get_parameter('lookahead_distance').value,
            'goal_threshold': self.get_parameter('goal_threshold').value,
            'orientation_threshold': self.get_parameter('orientation_threshold').value,
            'base_frame': self.get_parameter('base_frame').value,
            'global_frame': self.get_parameter('global_frame').value
        }
        
        # 创建回调组
        self.callback_group = ReentrantCallbackGroup()
        
        # 创建ROS接口
        self.ros_interface = ROSInterface(self, self.callback_group)
        
        # 创建路径跟随算法
        self.algorithm = VectorPathFollowingAlgorithm(params)
        
        # 注册回调函数
        self.ros_interface.set_path_callback(self.path_callback)
        
        # 创建定时器
        self.timer = self.create_timer(0.05, self.control_loop, callback_group=self.callback_group)
        
        self.get_logger().info('向量路径跟随节点已启动')
    
    def path_callback(self, path_msg):
        """路径消息回调函数"""
        self.get_logger().info('收到新路径，包含 {} 个路径点'.format(len(path_msg.poses)))
        self.algorithm.set_path(path_msg)
    
    def control_loop(self):
        """控制主循环"""
        # 获取当前位姿
        current_pose = self.ros_interface.get_current_pose()
        if current_pose is None:
            return
            
        # 计算控制命令
        linear_vel, angular_vel = self.algorithm.calculate_control(current_pose)
        
        # 发布速度命令
        self.ros_interface.publish_velocity(linear_vel, angular_vel)

def main(args=None):
    rclpy.init(args=args)
    node = VectorPathFollower()
    
    try:
        executor = MultiThreadedExecutor()
        rclpy.spin(node, executor)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()